#include <vector>
#include <math.h>

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "scan_process/descartes.h"
#include <tf/transform_broadcaster.h>

#define RAD2DEG(x) ((x)*180./M_PI)
#define DEG2RAD(x) ((x)/180.*M_PI)

using namespace std;

struct Index
{
    int num;
    float ranges;
};

class Scan_process
{
public:
    Scan_process();
    ros::NodeHandle nh;
    ros::Publisher scan_pub;
    ros::Subscriber sub;
    ros::ServiceClient ols_client;
    
    tf::TransformBroadcaster tf_broadcaster;
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);


private:
    /* data */
};

Scan_process::Scan_process(){
    sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1000, &Scan_process::scanCallback,this);
    scan_pub = nh.advertise<sensor_msgs::LaserScan>("p_scan", 1000);
    ols_client = nh.serviceClient<scan_process::descartes>("/ols");
    ros::spin();
}


void Scan_process::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    //临时存储分割点
    vector<Index> set;
    //存储每一段分割数据
    std::vector<Index> v[30];
    int vcount = 0;
    
    //点云分割
    for (size_t i = 1; i < 180; i++)
    {
        //范围  0.1-8.0米,超过这个范围不检测
        if (scan->ranges[i] < 0.1|| scan->ranges[i] > 8.0){
            //set.clear();
            continue;
        }
        //获得个点与上一点的偏差
        float tem = fabs(scan->ranges[i] - scan->ranges[i-1]);
        
        //如果本次点在3m以内并且与上个点差值小于0.1m
        if ((scan->ranges[i] < 3.0) && tem < 0.1)
        {
            Index t;
            t.num = i;
            t.ranges = scan->ranges[i];
            set.push_back(t);
        //如果本次点大于3m并且与上个点差值小于0.2m
        }else if ((scan->ranges[i] > 3.0) && tem < 0.2){
            Index t;
            t.num = i;
            t.ranges = scan->ranges[i];
            set.push_back(t);
        //该段点云断掉
        }else{

            //如果该段点云小于七个则不计入,大于七将其放入v[]容器里面
            if(set.size() > 7){
                for (size_t j = 0; j < set.size(); j++)
                {
                    v[vcount].push_back(set[j]);
                }
                vcount++;
            }
            //临时存储清空
            set.clear();
        }
    }

    //对分割的点云进行筛选
    for (size_t i = 0; i < vcount; i++)
    {
        //t用来存储一段点云中距离最大的点
        Index t;
        t.ranges = 0;
        //查找最大的点
        for (size_t j = 0; j < v[i].size(); j++)
        {
            if (v[i][j].ranges > t.ranges)
            {
                t.ranges = v[i][j].ranges; 
                t.num = v[i][j].num;
            }
        }
        //判断最大点是否在中间
        if (t.num < (int)((v[i][0].num+3.0*v[i][v[i].size()-1].num)/4.0) && t.num > (int)((3.0 * v[i][0].num + v[i][v[i].size()-1].num)/4.0) )
        {
            //cout << i << "begin : " << v[i][0].num << " mind: " << t.num << " end : " << v[i][v[i].size()-1].num << endl;
            //定义两个分别存储点云从最大点分割的两组
            scan_process::descartes srv[2];
            for (size_t j = 0; j < v[i].size(); j++)
            {
                float angle = DEG2RAD((float)(v[i][j].num-90));
                if (v[i][j].num>t.num){
                    srv[0].request.x.push_back(v[i][j].ranges * cos(angle));
                    srv[0].request.y.push_back(v[i][j].ranges * sin(angle));
                }else{
                    srv[1].request.x.push_back(v[i][j].ranges * cos(angle));
                    srv[1].request.y.push_back(v[i][j].ranges * sin(angle));
                }
            }

            //对srv[0]进行最小二乘
            if (ols_client.call(srv[0]))
            {
                //对srv[1]进行最小二乘
                if (ols_client.call(srv[1])){
                    float k1,k2;
                    k1 = srv[0].response.k;
                    k2 = srv[1].response.k;
                    //求出两个一次函数的夹角
                    float angle = atan((k1-k2)/(1+k1*k2));

                    //判断其是否在110°~130°之间
                    if (RAD2DEG(angle)>50 && RAD2DEG(angle)<70){
                        // cout << " i :"<< i << "  k1 : " << k1 <<  "  k2 : " << k2 ;
                        // cout << "angle : "  << RAD2DEG(angle) << endl;
                        cout << endl;

                        float midline = sqrt(2*(v[i][0].ranges*v[i][0].ranges + v[i][v[i].size()-1].ranges*v[i][v[i].size()-1].ranges) - 0.27*0.27)/2.0;
                        cout <<" min " << midline << " a : " << v[i][0].ranges <<endl;

                        float cosa = (midline*midline + v[i][0].ranges*v[i][0].ranges - 0.135*0.135)/(2.0*midline*v[i][0].ranges);

                        float yaw = acos(cosa);
                        cout <<" cosa " << cosa << " yaw : " << yaw <<endl;
                        if (yaw>M_PI/2.0)
                            yaw -= M_PI/2.0;
                        else
                            yaw = M_PI/2.0 - yaw;

                        //充电桩角度
                        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(yaw);

                        geometry_msgs::TransformStamped tf_trans;
                        tf_trans.header.stamp = ros::Time::now();
                        tf_trans.header.frame_id = "laser_radar_Link";
                        tf_trans.child_frame_id = "charging";

                        tf_trans.transform.translation.x = t.ranges * cos(DEG2RAD(t.num-90)) ;
                        tf_trans.transform.translation.y = t.ranges * sin(DEG2RAD(t.num-90)) ;
                        tf_trans.transform.translation.z = 0.0;
                        tf_trans.transform.rotation = odom_quat;

                        //send the transform
                        tf_broadcaster.sendTransform(tf_trans);

                    }else{
                        cout << "angle : "  << RAD2DEG(angle) << endl;
                    }
                }
            }else{
                ROS_ERROR("ols server not found!");
            }
        }
    }

    //将分隔的点云发布出去
    sensor_msgs::LaserScan scan_msg;
    scan_msg.header.stamp = ros::Time::now();
    scan_msg.header.frame_id = "laser_radar_Link";
    scan_msg.header.stamp = ros::Time::now();
    scan_msg.angle_min = scan->angle_min;
    scan_msg.angle_max = scan->angle_max;

    scan_msg.angle_increment = scan->angle_increment;
    scan_msg.scan_time = scan->scan_time;
    scan_msg.time_increment = scan->time_increment;
    scan_msg.range_min = scan->range_min*2;
    scan_msg.range_max = scan->range_max*2;//8.0;

    int countv = 0;
    while (countv < vcount+1)
    {
        //cout <<" cv:" << countv <<" size:" << v[countv].size() ;
        for (size_t i = 0; i < v[countv].size(); i++)
        {
            scan_msg.ranges.push_back(v[countv][i].ranges);
        }
        v[countv].clear();
        scan_msg.ranges.push_back(1.0);
        scan_msg.ranges.push_back(1.0);
        scan_msg.ranges.push_back(1.0);
        countv++;
        if (v[countv].size() < 7) break;
    }
        //cout << " --endl" <<endl;

     scan_pub.publish(scan_msg);

  
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ydlidar_client");
    Scan_process sc;
    return 0;
}
